#include "main.h"
#include "utm_transformer.h"

C_apiBulidMap::C_apiBulidMap(C_apiCommunicate *C_apiCommunicate)
{
    p_C_apiCommunicate = C_apiCommunicate;

    minClosedCurveDistance = 50; // 配置建图时候的闭合曲线的最短距离为50cm
    mixCurveDistance = 100; // 设置开始建图的最小行走距离 100cm
}

C_apiBulidMap::~C_apiBulidMap()
{

}

void C_apiBulidMap::startLoop(void)
{
    thread1 = std::thread(&C_apiBulidMap::bulidMapLoop, this); 
    thread2 = std::thread(&C_apiBulidMap::getMessage, this); 
}

int C_apiBulidMap::getMessage(void)
{
    eCAL::protobuf::CSubscriber<MowerProtoPakg::localizationGNGGA> subscriber("GNGGAMessage");
    subscriber.AddReceiveCallback(std::bind(&Callback, std::placeholders::_2));
    while (eCAL::Ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    eCAL::Finalize();
    return 0;
}

/***********************************************
 * @author: tianxiaohua
 * @date:   24/5
 * @brief:  回调函数，通过ecal订阅获取坐标的报文进行回调
 * @var:    lat_degree：地图的横坐标； lon_degree：地图的纵坐标
 * @return: void
 ***********************************************/
void C_apiBulidMap::bulidMapLoop(void)
{
    int workFlag = 0;
    COORDINATE tempCoordinate;
    eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> *p_ecal_bulidMapMessage;
    eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> ecal_bulidMapMessage("bulidMapMessage");
    p_ecal_bulidMapMessage = &ecal_bulidMapMessage;
    int closedFlag = 0;
    while(1){
        /**
         * 0、等待外部信号是否开始建图
         * 1、接收获取到的经纬度数据
         * 2、处理经纬度数据，转换成世界坐标系
         * 3、将每条数据原始数据都到buf中，逐渐增加
         * 4、完成建图，把经纬度和坐标分别储存到文件中
        */
        workFlag = p_C_apiCommunicate->getOrderBuild();
        // if(start == workFlag){ // 判断开始工作
            while(true){
                sleep(1);
                spdlog::info("C_apiBulidMap wait message");
                if(getFifoDate(tempCoordinate) == true){
                    saveCoordinate(tempCoordinate); // 保存坐标变量
                    /**发布坐标系广播*/
                    ecal_sandMessage(relativeCoordinateBuf.back().lat_degree, relativeCoordinateBuf.back().lon_degree, 
                                        earthCoordinateBuf[0].lat_degree, earthCoordinateBuf[0].lon_degree, p_ecal_bulidMapMessage); // 发布世界坐标系
                    switch (closedFlag)
                    {
                    case 0:{
                        /**判断路径的第一个点和最后一个点之间的距离，如果距离大于设定距离判定为建图的尺寸符合要求了*/
                        float distance = calculateDistance(relativeCoordinateBuf[0], relativeCoordinateBuf.back());
                        if(distance >= mixCurveDistance){
                            spdlog::error("C_apiBulidMap {} close curver distance is open!!!",distance);
                            closedFlag = 1;
                        }  break;
                    }
                    case 1:{
                        /**判断路径的第一个点和最后一个点之间的距离，如果距离小于设定距离判定为建图的曲线已经封闭了*/
                        float distance = calculateDistance(relativeCoordinateBuf[0], relativeCoordinateBuf.back());
                        if(distance <= minClosedCurveDistance){
                            spdlog::info("C_apiBulidMap {} close curver distance is closed!!!",distance);
                            
                        }break;
                    }
                    default:{}
                        break;
                    }
                                     
                    
                }
            }
        // }     
    }
}


int C_apiBulidMap::bulidMap(COORDINATE &pCoordinate, eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> *p_ecal_bulidMapMessage)
{
    return 0;
}

int C_apiBulidMap::saveCoordinate(COORDINATE &pCoordinate)
{
    COORDINATE tempPCoordinate;

    // spdlog::info("lat_degree {} lon_degree {} ",pCoordinate.lat_degree, pCoordinate.lon_degree);
    originalCoordinateBuf.push_back(pCoordinate); // 保存原始坐标数据

    std::pair<double, double> coordinate = from_latlon(pCoordinate.lat_degree, pCoordinate.lon_degree); // 经纬度数据转换为世界坐标系
    tempPCoordinate.lat_degree = coordinate.first; 
    tempPCoordinate.lon_degree = coordinate.second;
    earthCoordinateBuf.push_back(tempPCoordinate); // 保存世界坐标数据
    // spdlog::info("lat_degree {} lon_degree {} ",tempPCoordinate.lat_degree, tempPCoordinate.lon_degree);
    tempPCoordinate.lat_degree = (tempPCoordinate.lat_degree - earthCoordinateBuf[0].lat_degree) * 100; // 厘米单位的相对坐标系. 相对于第一个点
    tempPCoordinate.lon_degree = (tempPCoordinate.lon_degree - earthCoordinateBuf[0].lon_degree) * 100;
    relativeCoordinateBuf.push_back(tempPCoordinate); // 相对坐标系
    // spdlog::info("lat_degree {} lon_degree {} ",tempPCoordinate.lat_degree, tempPCoordinate.lon_degree);
    return 0;
}



/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  计算两点之间的距离
 * @var:    point1第一个点，point2下一个目标点
 * @return: 两个点之间的距离
 ***********************************************/
double C_apiBulidMap::calculateDistance(COORDINATE &point1, COORDINATE &point2) 
{
    double x_distance = point1.lat_degree - point2.lat_degree;
    double y_distance = point1.lon_degree - point2.lon_degree;
    return std::sqrt(x_distance*x_distance + y_distance*y_distance); // 计算开平方根
}


/***********************************************
 * @author: tianxiaohua
 * @date:   24/5
 * @brief:  把接收到的定位数据转换后的世界坐标系数据和原点数据广播出去
 * @var:    lat_degree：地图的横坐标； lon_degree：地图的纵坐标
 * @return: void
 ***********************************************/
void C_apiBulidMap::ecal_sandMessage(double lat_degree, double lon_degree, double start_lat_degree, double start_lon_degree, eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> *p_ecal_bulidMapMessage)
{
    // 把数据包装成protobuf
    MowerProtoPakg::bulidMapLocalization message;// 创建一个protobuf消息对象
    message.set_wordx(lat_degree);
    message.set_wordy(lon_degree);
    message.set_startx(start_lat_degree);
    message.set_starty(start_lon_degree);
    // Send the message
    p_ecal_bulidMapMessage->Send(message);
}

/***********************************************
 * @author: tianxiaohua
 * @date:   24/5
 * @brief:  回调函数，通过ecal订阅获取坐标的报文进行回调
 * @var:    lat_degree：地图的横坐标； lon_degree：地图的纵坐标
 * @return: void
 ***********************************************/
void C_apiBulidMap::Callback(const MowerProtoPakg::localizationGNGGA& msg) 
{
    // printf("time:%f\n", msg.time());
    fifoSaveDate(msg.latitude(), msg.longitude());
}

/***********************************************
 * @author: tianxiaohua
 * @date:   24/5
 * @brief:  回调函数，通过ecal订阅获取坐标的报文进行回调
 * @var:    lat_degree：地图的横坐标； lon_degree：地图的纵坐标
 * @return: void
 ***********************************************/
int C_apiBulidMap::saveLongitudeLatitudeToFile(std::vector<std::string> buf) // 经纬度信息保存到文件里
{
    return false;
}

/***********************************************
 * @author: tianxiaohua
 * @date:   23/10
 * @brief:  通过一个vector buf来转存回调函数中接收到的数据，形成一个fifo结构。防止数据丢失处理不过来，
 * @var:    经纬度数据
 * @return: void
 ***********************************************/
std::vector<COORDINATE> DataBuf;
void C_apiBulidMap::fifoSaveDate(double lat_degree, double lon_degree)
{
    extern std::vector<COORDINATE> DataBuf;
    COORDINATE tempCoordinate;
    // printf("经度:%f\n", lat_degree);
    // printf("纬度:%f\n", lon_degree);
    tempCoordinate.lat_degree = lat_degree;
    tempCoordinate.lon_degree = lon_degree;
    DataBuf.push_back(tempCoordinate);
} 

/***********************************************
 * @author: tianxiaohua
 * @date:   23/10
 * @brief:  获取fifo中转存的数据内容，按照顺序输出；
 * @var:    经纬度数据结构体
 * @return: 获取成功和失败的标志
 ***********************************************/
bool C_apiBulidMap::getFifoDate(COORDINATE &tempCoordinate)
{
    int vertorLen = DataBuf.size();
    if(vertorLen == 0){
        return false;
    }
    else{
        tempCoordinate = DataBuf[0];
        DataBuf.erase(DataBuf.begin());
        return true;
    }
    return false;
}

